#include "SimpleDrawer.h"
#include "util/TIMER.h"
#include <boost/filesystem.hpp>
#include <pangolin/pangolin.h>
#include <algorithm>


namespace Viewer
{
void SimpleDrawer::SetPoses(std::vector<std::array<double,12> >& vPoses)
{
    mvMatrix4d_.reserve(vPoses.size());
    std::for_each(mvMatrix4d_.begin(), mvMatrix4d_.end(), [](Eigen::Matrix4d& matrix){
        matrix = Eigen::Matrix4d::Identity();
    });

    std::transform(vPoses.begin(), vPoses.end(), std::back_inserter(mvMatrix4d_), [](std::array<double,12>& pose)-> Eigen::Matrix4d{
        Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> > Transform(pose.data());
        Eigen::Matrix4d temp = Eigen::Matrix4d::Identity();
        temp.topLeftCorner<3,4>() = Transform;
        return temp;
    });

    // std::copy(mvMatrix4d_.begin(), mvMatrix4d_.end(), std::ostream_iterator<Eigen::Matrix4d>(std::cout, "\n"));
    
}


void SimpleDrawer::Run()
{
    pangolin::CreateWindowAndBind("Main",640,480);
    Viewer::SimpleDrawer simpleDrawer;
    glEnable(GL_DEPTH_TEST);

    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,100),
        pangolin::ModelViewLookAt(-2,2,-2, 0,0,0, pangolin::AxisY)
    );

    // Create Interactive View in window
    pangolin::Handler3D handler(s_cam);
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
            .SetHandler(&handler);

    size_t i = 0;
    Estimator::TIMER timer("count");
    timer.start_timer();
    while( !pangolin::ShouldQuit() )
    {
        timer.end_timer();
        //std::cout << timer.duration_timer() << std::endl;
        if(timer.duration_timer() >= 3.0)
        {
            i++;
            std::cout << i << std::endl;
            timer.start_timer();
        }
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        // pangolin::GlFont::I().Text("Hello World!").Draw(0,0,0);
        // text.Draw(0,0,0);
        // Render OpenGL Cube
        std::for_each(mvvMatrix4d_[i].cbegin(), mvvMatrix4d_[i].cend(),[&simpleDrawer](Eigen::Matrix4d matrix4d){
            simpleDrawer.DrawCamera(matrix4d);
        });
        // simpleDrawer.DrawCamera(Eigen::Matrix4d::Identity());
        // Eigen::Vector3d rev(3.0,5.0,2.3);
        // Eigen::Matrix4d temp = Eigen::Matrix4d::Identity();
        // temp.topRightCorner<3,1>() = rev;
        // simpleDrawer.DrawCamera(temp);
        // Swap frames and Process Events
        pangolin::FinishFrame();
    }
}

void SimpleDrawer::SetAllPoses(std::vector< std::vector<std::array<double,12> > >& vvPoses)
{
    std::for_each(vvPoses.begin(), vvPoses.end(), [this](std::vector<std::array<double,12> > poses){
        this->SetPoses(poses);
        this->mvvMatrix4d_.push_back(this->mvMatrix4d_);
    });
    //std::cout << this->mvvMatrix4d_.back().back() << std::endl;

    std::cout << "Here is the 6 iteration poses: " << std::endl;
    // std::copy(this->mvvMatrix4d_[6].begin(), this->mvvMatrix4d_[6].end(), std::ostream_iterator<Eigen::Matrix4d>(std::cout, "\n"));
}

void SimpleDrawer::DrawCamera(Eigen::Matrix<double,4,4> mat )
{
    const float &w = 0.8;
    const float h = w*0.75;
    const float z = w*0.6;
    pangolin::OpenGlMatrix Twc(mat);

    glPushMatrix();
    glMultMatrixd(Twc.m);
    glLineWidth(3);
    glColor3f(0.0f, 0.0f, 1.0f);
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(w, h, z);
    glVertex3f(0, 0, 0);
    glVertex3f(w, -h, z);
    glVertex3f(0, 0, 0);
    glVertex3f(-w, -h, z);
    glVertex3f(0, 0, 0);
    glVertex3f(-w, h, z);

    glVertex3f(w, h, z);
    glVertex3f(w, -h, z);

    glVertex3f(-w, h, z);
    glVertex3f(-w, -h, z);

    glVertex3f(-w, h, z);
    glVertex3f(w, h, z);

    glVertex3f(-w, -h, z);
    glVertex3f(w, -h, z);
    glEnd();

    glPopMatrix();
}
}